
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       p_1d.h
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    // parameters
    float _kp;

    // internal variables
    float _dt;          // time step in seconds
    float _error;       // time step in seconds
    float _error_min;   // error limit in negative direction
    float _error_max;   // error limit in positive direction
    float _D1_max;      // maximum first derivative of output
} P_1d_ctrl;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// constructor
void p_1d_ctrl_ctor(P_1d_ctrl* controler, float initial_p, float dt);

// set time step in seconds
static inline void p_1d_ctrl_set_dt(P_1d_ctrl* controler, float dt) { controler->_dt = dt; }

// update_all - set target and measured inputs to P controller and calculate outputs
// target and measurement are filtered
// if measurement is further than error_min or error_max (see set_limits method)
//   the target is moved closer to the measurement and limit_min or limit_max will be set true
float p_1d_ctrl_update_all(P_1d_ctrl* controler, float *target, float measurement, bool *limit_min, bool *limit_max);

// set_limits - sets the maximum error to limit output and first and second derivative of output
void p_1d_ctrl_set_limits(P_1d_ctrl* controler, float output_min, float output_max, float D_Out_max, float D2_Out_max);

// set_error_limits - reduce maximum position error to error_max
// to be called after setting limits
void p_1d_ctrl_set_error_limits(P_1d_ctrl* controler, float error_min, float error_max);

// get_error_min - return minimum position error
static inline float p_1d_ctrl_get_error_min(P_1d_ctrl* controler) { return controler->_error_min; }

// get_error_max - return maximum position error
static inline float p_1d_ctrl_get_error_max(P_1d_ctrl* controler) { return controler->_error_max; }

// save gain to eeprom
//static inline void p_1d_ctrl_save_gains(P_1d_ctrl controler) { _kp.save(); }

// accessors
static inline float p_1d_ctrl_get_kp(P_1d_ctrl* controler) { return controler->_kp; }
static inline float p_1d_ctrl_get_error(P_1d_ctrl* controler) { return controler->_error; }

// set accessors
static inline void p_1d_ctrl_set_kp(P_1d_ctrl* controler, float v) { controler->_kp = v; }
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



